package jmotioncapture.calculation;

import java.util.Iterator;
import java.util.Vector;

import jmotioncapture.geometry2D.Line;
import jmotioncapture.geometry2D.Point;
import jmotioncapture.geometry2D.PointComparable;
import jmotioncapture.util.Mathematics;
import jmotioncapture.util.Sort;

public class GeomCalculation {

	/**
	 * Get the distance between two points.
	 * 
	 * @param opt1
	 *            Point A
	 * @param opt2
	 *            Point B
	 * @return Distance in double
	 */
	public static double distance(Point pt1, Point pt2) {
		try {
			return Math
					.sqrt((Mathematics.square(pt1.getX() - pt2.getX()) + Mathematics
							.square(pt1.getY() - pt2.getY())));
		} catch (RuntimeException e) {
			return Double.NaN;
		}
	}

	/**
	 * Get the mid point of point 1 and point 2
	 * 
	 * @param pt1
	 *            Point 1
	 * @param pt2
	 *            Point 2
	 * @return Mid-point
	 */
	public static Point midPoint(Point pt1, Point pt2) {

		try {
			return new Point((short) ((pt1.getX() + pt2.getX()) / 2), (short) ((pt1
					.getY() + pt2.getY()) / 2));
		} catch (RuntimeException e) {
			e.printStackTrace();
			return null;
		}
	}

	/**
	 * Get the area with four points
	 * 
	 * @param opt
	 *            A Point array
	 * @return Area in double
	 */
	public static double fourPointArea(Vector<Point> opt) {
		try {
			double area = 0;
			Vector<Point> temp = new Vector<Point>(3, 1);

			// 012
			temp.add(opt.elementAt(0));
			temp.add(opt.elementAt(1));
			temp.add(opt.elementAt(2));
			double area1 = threePointArea(temp);

			// 023
			temp.add(opt.elementAt(0));
			temp.add(opt.elementAt(2));
			temp.add(opt.elementAt(3));
			double area2 = threePointArea(temp);

			// 013
			temp.add(opt.elementAt(0));
			temp.add(opt.elementAt(1));
			temp.add(opt.elementAt(3));
			double area3 = threePointArea(temp);

			// 123
			temp.add(opt.elementAt(1));
			temp.add(opt.elementAt(2));
			temp.add(opt.elementAt(3));
			double area4 = threePointArea(temp);

			area = (area1 + area2 + area3 + area4) / 2;

			return area;
		} catch (Exception exc) {
			return Double.NaN;
		}

	}

	/**
	 * Get the area formed by three points
	 * 
	 * @param opt
	 *            A Point vector array with 3 elements
	 * @return Area in double
	 */
	public static double threePointArea(Vector<Point> opt) {

		if (opt.size() == 3) {

			double a = distance(opt.elementAt(1), opt.elementAt(2));
			double b = distance(opt.elementAt(2), opt.elementAt(0));
			double c = distance(opt.elementAt(1), opt.elementAt(0));

			double i = Mathematics.square(Mathematics.square(a)
					+ Mathematics.square(b) + Mathematics.square(c));
			double j = 2 * (Math.pow(a, 4) + Math.pow(b, 4) + Math.pow(c, 4));

			return (0.25 * (Math.pow(i - j, 0.5)));
		}
		return Double.NaN;
	}

	/**
	 * Get the area formed by three points
	 * 
	 * @param p1
	 * @param p2
	 * @param p3
	 * @return
	 */
	public static double threePointArea(Point p1, Point p2, Point p3) {
		double a = distance(p1, p2);
		double b = distance(p2, p3);
		double c = distance(p1, p3);

		double i = Mathematics.square(Mathematics.square(a)
				+ Mathematics.square(b) + Mathematics.square(c));
		double j = 2 * (Math.pow(a, 4) + Math.pow(b, 4) + Math.pow(c, 4));

		return (0.25 * (Math.pow(i - j, 0.5)));
	}

	/**
	 * Get the maximun area bounded by n points
	 * @param opt
	 * @param isOutterBoundPoints
	 * @return
	 */
	public static double nPointsArea(Vector<Point> opt, boolean isOutterBoundPoints) {
		double area = 0;

		if(!isOutterBoundPoints)
			opt = giftWrappingBound(opt);

		if (opt.size() < 3)
			return Double.NaN;

		for (int i = 0; i < opt.size() - 2; i++)
			area += threePointArea(opt.elementAt(i), opt.elementAt(i + 1), opt
					.elementAt(i + 2));

		return area;
	}

	/**
	 * Get the intersection point of the two diagonal lines formed by two lines
	 * 
	 * @param line1
	 *            First Line
	 * @param line2
	 *            Second Line
	 * @return Intersection Point in Point
	 */
	public static Point intersectionPointOfTwoLines(Line line1, Line line2) {
		return intersectionPointOfTwoLines(line1.getStartPoint(), line1
				.getEndPoint(), line2.getStartPoint(), line2.getEndPoint());
	}

	
	/**
	 * Get the intersection point of the two diagonal lines formed by four
	 * points
	 * 
	 * @param pt1
	 * @param pt2
	 * @param pt3
	 * @param pt4
	 * @return
	 */
	public static Point intersectionPointOfTwoLines(Point pt1, Point pt2,
			Point pt3, Point pt4) {

		try {
			double pux = ((pt3.getX() - pt1.getX()) * (pt2.getY() - pt1.getY()) - (pt3
					.getY() - pt1.getY())
					* (pt2.getX() - pt1.getX()))
					/ ((pt3.getY() - pt1.getY()) * (pt4.getX() - pt2.getX()) - (pt3
							.getX() - pt1.getX())
							* (pt4.getY() - pt2.getY()));

			double pmx = pt2.getX() + pux * (pt4.getX() - pt2.getX());

			double puy = ((pt4.getX() - pt2.getX()) * (pt2.getY() - pt1.getY()) - (pt4
					.getY() - pt2.getY())
					* (pt2.getX() - pt1.getX()))
					/ ((pt3.getY() - pt1.getY()) * (pt4.getX() - pt2.getX()) - (pt3
							.getX() - pt1.getX())
							* (pt4.getY() - pt2.getY()));

			double pmy = pt2.getY() + puy * (pt4.getY() - pt2.getY());

			return new Point((short) pmx, (short) pmy);
		} catch (ArithmeticException e) {
			return null;
		}
	}

	/**
	 * Get the slope of the line
	 * 
	 * @param opt1
	 *            Point 1
	 * @param opt2
	 *            Point 2
	 * @return Slope in double
	 */
	public static double slope(Point opt1, Point opt2) {
		try {
			double v1 = (double) (opt2.getY() - opt1.getY());
			double v2 = (double) (opt2.getX() - opt1.getX());
			return (-1.0 * v1 / v2);
		} catch (RuntimeException e) {
			return Double.NaN;
		}
	}

	/**
	 * Get the left most point
	 * 
	 * @param pts
	 *            Point Set
	 * @return left most point
	 */
	public static Point getLeftMostPoint(Vector<Point> pts) {
		Iterator<Point> itPt = null;
		Point pt = null, leftPt = null;

		double minX = Double.MAX_VALUE;

		itPt = pts.iterator();
		while (itPt.hasNext()) {
			pt = itPt.next();
			if (pt.getX() < minX) {
				minX = pt.getX();
				leftPt = pt;
			}
		}
		return leftPt;
	}

	/**
	 * Get the right most point
	 * 
	 * @param pts
	 *            Point Set
	 * @return left most point
	 */
	public static Point getRightMostPoint(Vector<Point> pts) {
		Iterator<Point> itPt = null;
		Point pt = null, leftPt = null;

		double maxX = Double.MIN_VALUE;

		itPt = pts.iterator();
		while (itPt.hasNext()) {
			pt = itPt.next();
			if (pt.getX() > maxX) {
				maxX = pt.getX();
				leftPt = pt;
			}
		}
		return leftPt;
	}

	@SuppressWarnings("unchecked")
	public static PointComparable[] sortPointsByX(Vector<Point> pts) {
		PointComparable[] ptcs = new PointComparable[pts.size()];

		for (int i = 0; i < ptcs.length; i++)
			ptcs[i] = new PointComparable(pts.elementAt(i),
					PointComparable.SORTING_BY_X);

		return (PointComparable[]) Sort.quickSort(ptcs);
	}

	@SuppressWarnings("unchecked")
	public static PointComparable[] sortPointsByY(Vector<Point> pts) {
		PointComparable[] ptcs = new PointComparable[pts.size()];

		for (int i = 0; i < ptcs.length; i++)
			ptcs[i] = new PointComparable(pts.elementAt(i),
					PointComparable.SORTING_BY_Y);

		return (PointComparable[]) Sort.quickSort(ptcs);
	}

	/**
	 * Get the points bounding the whole point set, order by clockwise
	 * 
	 * @param pts
	 *            Point Set
	 * @return Point set in Point Vector
	 */
	public static Vector<Point> giftWrappingBound(Vector<Point> pts) {

		if (pts.size() < 3)
			return new Vector<Point>();

		Vector<Point> ptBound = new Vector<Point>();
		Vector<Point> tempPt = new Vector<Point>();
		Iterator<Point> itPt = null;
		Point currPt = null, initPt = null, rightPt = null;

		// Clone a temp object
		itPt = pts.iterator();
		while (itPt.hasNext())
			tempPt.add(itPt.next());

		// Gift Wrapping Algorithm to find the bounding rectangle
		// 1. Find leftmost point
		initPt = getLeftMostPoint(tempPt);
		if (initPt == null)
			return new Vector<Point>();
		ptBound.add(initPt);

		// 2. Choose the second point
		itPt = tempPt.iterator();
		double maxSlope = Double.NEGATIVE_INFINITY;
		while (itPt.hasNext()) {
			currPt = itPt.next();
			if (initPt != currPt) {
				double slope = slope(initPt, currPt);
				if (slope > maxSlope) {
					maxSlope = slope;
					rightPt = currPt;
				}
			}
		}
		ptBound.add(rightPt);
		tempPt.removeElement(rightPt);
		
		// 3. Find a point such that all other points are to the right of the
		// line
		boolean done = false;
		// Go thru every possible points
		while (tempPt.size() > 0 && !done) {
			rightPt = null;
			itPt = tempPt.iterator();

			double minAng = Double.MAX_VALUE;

			while (itPt.hasNext()) {
				currPt = itPt.next();
				double ang = Double.NaN;

				if (currPt != ptBound.elementAt(ptBound.size() - 2))
					ang = getAngleBetweenTwoLines(ptBound.elementAt(ptBound
							.size() - 2), ptBound.lastElement(), ptBound
							.lastElement(), currPt);

				// Need to external angle
				if (ang != Double.NaN) {
					ang = 360 - ang;
					if (ang != 0 && ang < minAng) {
						minAng = ang;
						rightPt = currPt;
					}
				}
			}

			// Add the point to list and move to other point
			if (rightPt != null) {
				if (rightPt == initPt)
					done = true;
				else {
					ptBound.add(rightPt);
					tempPt.removeElement(rightPt);
				}
			}
		}
		return ptBound;
	}

	/**
	 * Get the polar angle from optOrg to optTgt, when the x-axis has the angle
	 * of zero degree
	 * 
	 * @param line
	 *            Line
	 * @return Polar angle in double, range from 0 to 360
	 */
	public static double polarAngle(Line line) {
		return polarAngle(line.getStartPoint(), line.getEndPoint());
	}

	/**
	 * Get the polar angle from optOrg to optTgt, when the x-axis has the angle
	 * of zero degree
	 * 
	 * @param optOrg
	 *            Origin Point
	 * @param optTgt
	 *            Target Point
	 * @return Polar angle in double, range from 0 to 360
	 */
	public static double polarAngle(Point optOrg, Point optTgt) {
		double angrad = Math.atan2(optTgt.getY() - optOrg.getY(), optTgt.getX()
				- optOrg.getX());
		double degree = 0;
		if (angrad < 0)
			degree = Math.toDegrees(angrad + Math.PI * 2);
		else
			degree = Math.toDegrees(angrad);
		degree = (degree + 90) % 360;
		return degree;
	}

	/**
	 * Check if the two points is within the distance range.
	 * 
	 * @param pt1
	 *            Point A
	 * @param pt2
	 *            Point B
	 * @param distance
	 *            to check with
	 * @return true if two points is in range
	 */
	public static boolean withinDistance(Point pt1, Point pt2, double distance) {
		return Mathematics.square(pt1.getX() - pt2.getX())
				+ Mathematics.square(pt1.getY() - pt2.getY()) <= Mathematics
				.square(distance);
	}

	/**
	 * Get the speed in pixel per minisecond
	 * 
	 * @param pt1
	 * @param pt2
	 * @param fps
	 * @return
	 */
	public static double speed(Point pt1, Point pt2, long fps) {
		return (GeomCalculation.distance(pt1, pt2) * fps) / 1000;
	}

	/**
	 * Get a Vector<Line> that contains all combination of lines formed by a
	 * set of points
	 * 
	 * @param pts
	 * @return
	 */
	public static Vector<Line> getLines(Vector<Point> pts) {
		Vector<Line> lines = new Vector<Line>(10, 5);

		for (int i = 0; i < pts.size() - 1; i++)
			for (int j = i + 1; j < pts.size(); j++)
				lines.add(new Line(pts.elementAt(i), pts.elementAt(j)));

		return lines;
	}

	/**
	 * Get the polar angle between 2 lines. The angle is less than 180 degree.
	 * 
	 * @param line1
	 *            First Line
	 * @param line2
	 *            Second Line
	 * @return Polar angle in double, range from 0 to 180
	 */
	public static double getAngleBetweenTwoLines(Line line1, Line line2) {
		return getAngleBetweenTwoLines(line1.getStartPoint(), line1
				.getEndPoint(), line2.getStartPoint(), line2.getEndPoint());
	}

	/**
	 * Get the polar angle between 2 lines. The angle is less than 180 degree.
	 * 
	 * @param pt1
	 *            First point of line 1
	 * @param pt2
	 *            Second point of line 1
	 * @param pt3
	 *            First point of line 2
	 * @param pt4
	 *            Second point of line 2
	 * @return Polar angle in double, range from 0 to 180
	 */
	public static double getAngleBetweenTwoLines(Point pt1, Point pt2,
			Point pt3, Point pt4) {
		double temp = Math.abs(polarAngle(pt1, pt2) - polarAngle(pt3, pt4));
		return (180 - temp);
	}
}
